#ifndef GLOBAL_PLANNER_H
#define GLOBAL_PLANNER_H

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/MarkerArray.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Path.h>

#include "quad_msgs/Trajectory.h"
#include "quad_msgs/GlobalTrajProcess.h"

#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>

#include <pcl/io/pcd_io.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/kdtree/kdtree_flann.h>

#include "global_planner/ompl_planner.h"
#include "trajectory_optimization/trajectory_optimization.h"
#include "trajectory_optimization/bspline_smooth.h"

namespace Planner
{
    using std::string;

    class GlobalPlanner
    {
    public:
        GlobalPlanner(const ros::NodeHandle &node_handle);
        ~GlobalPlanner() = default;

        void init();

        void waypointCallback(const geometry_msgs::PoseStamped &pt);

        bool trajProcessCallback(quad_msgs::GlobalTrajProcess::Request &request,
                                 quad_msgs::GlobalTrajProcess::Response &response);

    private:
        ros::NodeHandle nh_;

        ros::Subscriber waypoint_sub_;
        ros::Publisher waypoints_pub_;
        ros::Publisher origin_path_pub_;
        ros::Publisher smooth_traj_pub_;
        ros::Publisher sample_traj_pub_;
        ros::Publisher optimized_traj_pub_;
        ros::Publisher cloud_map_pub_;

        ros::ServiceServer traj_process_server_;

        OMPLPlanner ompl_planner_;
        TrajectoryOptimization traj_opt_;
        BsplineSmooth bspline_smooth_;

        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_map_;
        pcl::KdTreeFLANN<pcl::PointXYZ>::Ptr kdtree_;

        Vector3d start_pos_, goal_pos_;
        vector<Vector3d> waypoints_, origin_path_, smooth_path_, sample_path_;

        vector<MatrixXd> coeffs_;
        vector<double> time_seq_;
        string traj_topic_;
        int opt_seg_num_;

        enum Status
        {
            INIT_FAIL,
            INIT_SUCCESS,
            SET_START,
            SET_GOAL
        };
        int status_;

        enum Ops
        {
            ADD_WAYPOINTS,
            SAMPLE_WAYPOINTS,
            OPTIMIZE_TRAJ,
            OPTIMIZE_FAIL
        };
        int ops_;

        // 对RRT*规划出的离散路径进行B样条平滑处理，再进行离散采样
        void samplePath();

        // 将采样后的路径点生成时域连续分段多项式轨迹
        void optimizeTraj();
    };
} // namespace Planner

#endif